// ************************************************************************************************************
// LCD & display & monitoring
// ************************************************************************************************************
#ifdef LCD_CONF

typedef void (*formatter_func_ptr) (void *, uint8_t, uint8_t);
typedef void (*inc_func_ptr) (void *, int8_t);

typedef struct lcd_type_desc_t {
    formatter_func_ptr fmt;
    inc_func_ptr inc;
};

static lcd_type_desc_t LTU8 = { &__u8Fmt, &__u8Inc };
static lcd_type_desc_t LTU16 = { &__u16Fmt, &__u16Inc };
static lcd_type_desc_t LPMM = { &__upMFmt, &__nullInc };
static lcd_type_desc_t LPMS = { &__upSFmt, &__nullInc };

typedef struct lcd_param_def_t {
    lcd_type_desc_t *type;
    uint8_t decimal;
    uint8_t multiplier;
    uint8_t increment;
};

typedef struct lcd_param_t {
    char *paramText;
    void *var;
    lcd_param_def_t *def;
};

// ************************************************************************************************************
// LCD Layout definition
// ************************************************************************************************************
// Descriptors
static lcd_param_def_t __P = { &LTU8, 1, 1, 1 };
static lcd_param_def_t __I = { &LTU8, 3, 1, 2 };
static lcd_param_def_t __D = { &LTU8, 0, 1, 1 };
static lcd_param_def_t __RCR = { &LTU8, 2, 2, 2 };
static lcd_param_def_t __RC = { &LTU8, 2, 1, 2 };
static lcd_param_def_t __PM = { &LPMM, 1, 1, 0 };
static lcd_param_def_t __PS = { &LPMS, 1, 1, 0 };
static lcd_param_def_t __PT = { &LTU8, 0, 1, 1 };
static lcd_param_def_t __VB = { &LTU8, 1, 1, 0 };
// Parameters
static lcd_param_t lcd_param[] = {
    {"PITCH&ROLL P", &P8[ROLL], &__P}
    , {"ROLL   P", &P8[ROLL], &__P}, {"ROLL   I", &I8[ROLL], &__I}, {"ROLL   D", &D8[ROLL], &__D}
    , {"PITCH  P", &P8[PITCH], &__P}, {"PITCH  I", &I8[PITCH], &__I}, {"PITCH  D", &D8[PITCH], &__D}
    , {"YAW    P", &P8[YAW], &__P}, {"YAW    I", &I8[YAW], &__I}, {"YAW    D", &D8[YAW], &__D}
#if BARO
    , {"ALT    P", &P8[PIDALT], &__P}, {"ALT    I", &I8[PIDALT], &__I}, {"ALT    D", &D8[PIDALT], &__D}
    , {"VEL    P", &P8[PIDVEL], &__P}, {"VEL    I", &I8[PIDVEL], &__I}, {"VEL    D", &D8[PIDVEL], &__D}
#endif
    , {"LEVEL  P", &P8[PIDLEVEL], &__P}, {"LEVEL  I", &I8[PIDLEVEL], &__I}
#if MAG
    , {"MAG    P", &P8[PIDMAG], &__P}
#endif
    , {"RC RATE", &rcRate8, &__RCR}
    , {"RC EXPO", &rcExpo8, &__RC}
    , {"PITCH&ROLL RATE", &rollPitchRate, &__RC}
    , {"YAW RATE", &yawRate, &__RC}
    , {"THROTTLE PID", &dynThrPID, &__RC}
#ifdef POWERMETER
    , {"pMeter Motor 0", &pMeter[0], &__PM}, {"pMeter Motor 1", &pMeter[1], &__PM}, {"pMeter Motor 2", &pMeter[2], &__PM}
#if (NUMBER_MOTOR > 3)
    , {"pMeter Motor 3", &pMeter[3], &__PM}
#endif
#if (NUMBER_MOTOR > 4)
    , {"pMeter Motor 4", &pMeter[4], &__PM}, {"pMeter Motor 5", &pMeter[5], &__PM}
#endif
#if (NUMBER_MOTOR > 6)
    , {"pMeter Motor 6", &pMeter[6], &__PM}, {"pMeter Motor 7", &pMeter[7], &__PM}
#endif
    , {"pMeter Sum", &pMeter[PMOTOR_SUM], &__PS}
    , {"pAlarm /50", &powerTrigger1, &__PT}	// change text to represent PLEVELSCALE value
#endif
#ifdef VBAT
    , {"Battery Volt", &vbat, &__VB}
#endif
};
#define PARAMMAX (sizeof(lcd_param)/sizeof(lcd_param_t) - 1)
// ************************************************************************************************************

// 1000000 / 9600  = 104 microseconds at 9600 baud.
// we set it below to take some margin with the running interrupts
#define BITDELAY 102
void LCDprint(uint8_t i)
{
#if defined(LCD_TEXTSTAR)
    Serial.print(i, BYTE);
#else
    LCDPIN_OFF delayMicroseconds(BITDELAY);
    for (uint8_t mask = 0x01; mask; mask <<= 1) {
	if (i & mask) {
	    LCDPIN_ON;
	} else {
	    LCDPIN_OFF;		// choose bit
	}
	delayMicroseconds(BITDELAY);
    }
    LCDPIN_ON;			//switch ON digital PIN 0
    delayMicroseconds(BITDELAY);
#endif
}

void LCDprintChar(const char *s)
{
    while (*s)
	LCDprint(*s++);
}

void initLCD()
{
    blinkLED(20, 30, 1);
#if defined(LCD_TEXTSTAR)
    // Cat's Whisker Technologies 'TextStar' Module CW-LCD-02
    // http://cats-whisker.com/resources/documents/cw-lcd-02_datasheet.pdf
    // Modified by Luca Brizzi aka gtrick90 @ RCG
    LCDprint(0xFE);
    LCDprint(0x43);
    LCDprint(0x02);		//cursor blink mode
    LCDprint(0x0c);		//clear screen
    LCDprintChar("MultiWii Config");
    LCDprint(0x0d);		// carriage return
    LCDprintChar("for all params");
    delay(2500);
    LCDprint(0x0c);		//clear screen
#else
    Serial.end();
    //init LCD
    PINMODE_LCD			//TX PIN for LCD = Arduino RX PIN (more convenient to connect a servo plug on arduino pro mini)
#endif
}

static char line1[17], line2[17];

void __u8Inc(void *var, int8_t inc)
{
    *(uint8_t *) var += inc;
};
void __u16Inc(void *var, int8_t inc)
{
    *(uint16_t *) var += inc;
};
void __nullInc(void *var, int8_t inc)
{
};

void __u8Fmt(void *var, uint8_t mul, uint8_t dec)
{
    uint16_t unit = *(uint8_t *) var;
    unit *= mul;
    char c1 = '0' + unit / 100;
    char c2 = '0' + unit / 10 - (unit / 100) * 10;
    char c3 = '0' + unit - (unit / 10) * 10;
    switch (dec) {
    case 0:
	line2[6] = c1;
	line2[7] = c2;
	line2[8] = c3;
	break;
    case 1:
	line2[5] = c1;
	line2[6] = c2;
	line2[7] = '.';
	line2[8] = c3;
	break;
    case 2:
	line2[5] = c1;
	line2[6] = '.';
	line2[7] = c2;
	line2[8] = c3;
	break;
    case 3:
	line2[4] = '0';
	line2[5] = '.';
	line2[6] = c1;
	line2[7] = c2;
	line2[8] = c3;
	break;
    }
}

void __u16Fmt(void *var, uint8_t mul, uint8_t dec)
{
    uint16_t unit = *(uint16_t *) var;
    unit *= mul;
    line2[4] = '0' + unit / 10000;
    line2[5] = '0' + unit / 1000 - (unit / 10000) * 10;
    line2[6] = '0' + unit / 100 - (unit / 1000) * 10;
    line2[7] = '0' + unit / 10 - (unit / 100) * 10;
    line2[8] = '0' + unit - (unit / 10) * 10;
}

void __upMFmt(void *var, uint8_t mul, uint8_t dec)
{
    uint32_t unit = *(uint32_t *) var;
    // pmeter values need special treatment, too many digits to fit standard 8 bit scheme
    unit = unit / PLEVELDIVSOFT;	// [0:1000] * 1000/3 samples per second(loop time) * 60 seconds *5 minutes -> [0:10000 e4] per motor
    // (that is full throttle for 5 minutes sampling with high sampling rate for wmp only)
    // times 6 for a maximum of 6 motors equals [0:60000 e4] for the sum
    // we are only interested in the big picture, so divide by 10.000
    __u16Fmt(&unit, mul, dec);
}

void __upSFmt(void *var, uint8_t mul, uint8_t dec)
{
    uint32_t unit = *(uint32_t *) var;
#if (POWERMETER == 1)
    unit = unit / PLEVELDIVSOFT;
#else
    unit = unit / PLEVELDIV;
#endif
    __u16Fmt(&unit, mul, dec);
}

static uint8_t lcdStickState[3];
#define IsLow(x)  (lcdStickState[x] & 0x1)
#define IsHigh(x) (lcdStickState[x] & 0x2)
#define IsMid(x)  (!lcdStickState[x])

void configurationLoop()
{
    uint8_t i, p;
    uint8_t LCD = 1;
    uint8_t refreshLCD = 1;
    uint8_t key = 0;
    initLCD();
    p = 0;
    while (LCD == 1) {
	if (refreshLCD) {
	    blinkLED(10, 20, 1);
	    strcpy(line2, "                ");
	    strcpy(line1, "                ");
	    i = 0;
	    char *point = lcd_param[p].paramText;
	    while (*point)
		line1[i++] = *point++;
	    lcd_param[p].def->type->fmt(lcd_param[p].var, lcd_param[p].def->multiplier, lcd_param[p].def->decimal);
#if defined(LCD_TEXTSTAR)
	    LCDprint(0xFE);
	    LCDprint('L');
	    LCDprint(1);
	    LCDprintChar(line1);	//refresh line 1 of LCD
	    LCDprint(0xFE);
	    LCDprint('L');
	    LCDprint(2);
	    LCDprintChar(line2);	//refresh line 2 of LCD
#else
	    LCDprint(0xFE);
	    LCDprint(128);
	    LCDprintChar(line1);
	    LCDprint(0xFE);
	    LCDprint(192);
	    LCDprintChar(line2);
#endif
	    refreshLCD = 0;
	}
#if defined(LCD_TEXTSTAR)
	key = (Serial.available()? Serial.read() : 0);
#endif
	for (i = ROLL; i < THROTTLE; i++) {
	    uint16_t Tmp = readRawRC(i);
	    lcdStickState[i] = (Tmp < MINCHECK) | ((Tmp > MAXCHECK) << 1);
	};
	if (IsLow(YAW) && IsHigh(PITCH))
	    LCD = 0;		// save and exit
	else if (IsHigh(YAW) && IsHigh(PITCH))
	    LCD = 2;		// exit without save: eeprom has only 100.000 write cycles
	else if (key == LCD_MENU_NEXT || (IsLow(PITCH))) {	//switch config param with pitch
	    refreshLCD = 1;
	    p++;
	    if (p > PARAMMAX)
		p = 0;
	} else if (key == LCD_MENU_PREV || (IsHigh(PITCH))) {
	    refreshLCD = 1;
	    p--;
	    if (p == 0xFF)
		p = PARAMMAX;
	} else if (key == LCD_VALUE_DOWN || (IsLow(ROLL))) {	//+ or - param with low and high roll
	    refreshLCD = 1;
	    lcd_param[p].def->type->inc(lcd_param[p].var, -lcd_param[p].def->increment);
	    if (p == 0)
		memcpy(lcd_param[4].var, lcd_param[0].var, 1);
	} else if (key == LCD_VALUE_UP || (IsHigh(ROLL))) {
	    refreshLCD = 1;
	    lcd_param[p].def->type->inc(lcd_param[p].var, +lcd_param[p].def->increment);
	    if (p == 0)
		memcpy(lcd_param[4].var, lcd_param[0].var, 1);
	}
    }				// while (LCD == 1)
    blinkLED(20, 30, 1);
#if defined(LCD_TEXTSTAR)
    LCDprint(0x0c);		//clear screen
    if (LCD == 0)
	LCDprintChar("Saving Settings..");
    else
	LCDprintChar("skipping Save.");
#endif
    if (LCD == 0)
	writeParams();
#if defined(LCD_TEXTSTAR)
    LCDprintChar("..done! Exit.");
#else
    Serial.begin(SERIAL_COM_SPEED);
#endif
#ifdef LCD_TELEMETRY
    delay(1000);		// keep exit message visible for one second even if (auto)telemetry continues writing in main loop
#endif
}
#endif


// -------------------- telemtry output to LCD over serial ----------------------------------

#ifdef LCD_TELEMETRY
void lcd_telemetry()
{
    // LCD_BAR(n,v) : draw a bar graph - n number of chars for width, v value in % to display
#ifdef LCD_TEXTSTAR
#define LCD_BAR(n,v) { LCDprint(0xFE);LCDprint('b');LCDprint(n);LCDprint(v); }
#else
#define LCD_BAR(n,v) {}		// add your own implementation here
#endif
    uint16_t intPowerMeterSum;

    switch (telemetry) {	// output telemetry data, if one of four modes is set
    case 'C':			// button C on Textstar LCD -> cycle time
	strcpy(line1, "Cycle    _____us");	//uin16_t cycleTime
	/*            0123456789.12345 */
	strcpy(line2, "[_____, _____]us");	//uin16_t cycleTimeMax
	line1[9] = '0' + cycleTime / 10000;
	line1[10] = '0' + cycleTime / 1000 - (cycleTime / 10000) * 10;
	line1[11] = '0' + cycleTime / 100 - (cycleTime / 1000) * 10;
	line1[12] = '0' + cycleTime / 10 - (cycleTime / 100) * 10;
	line1[13] = '0' + cycleTime - (cycleTime / 10) * 10;
#ifdef LOG_VALUES
	line2[1] = '0' + cycleTimeMin / 10000;
	line2[2] = '0' + cycleTimeMin / 1000 - (cycleTimeMin / 10000) * 10;
	line2[3] = '0' + cycleTimeMin / 100 - (cycleTimeMin / 1000) * 10;
	line2[4] = '0' + cycleTimeMin / 10 - (cycleTimeMin / 100) * 10;
	line2[5] = '0' + cycleTimeMin - (cycleTimeMin / 10) * 10;
	line2[8] = '0' + cycleTimeMax / 10000;
	line2[9] = '0' + cycleTimeMax / 1000 - (cycleTimeMax / 10000) * 10;
	line2[10] = '0' + cycleTimeMax / 100 - (cycleTimeMax / 1000) * 10;
	line2[11] = '0' + cycleTimeMax / 10 - (cycleTimeMax / 100) * 10;
	line2[12] = '0' + cycleTimeMax - (cycleTimeMax / 10) * 10;
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(2);
	LCDprintChar(line2);	//refresh line 2 of LCD
#endif
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(1);
	LCDprintChar(line1);	//refresh line 1 of LCD
	break;
    case 'B':			// button B on Textstar LCD -> Voltage, PowerSum and power alarm trigger value
	strcpy(line1, "__._V   _____mAh");	//uint8_t vbat, intPowerMeterSum
	/*            0123456789.12345 */
	//    (line2,".......  ......."); // intPowerMeterSum, intPowerTrigger1
#ifdef VBAT
	line1[0] = '0' + vbat / 100;
	line1[1] = '0' + vbat / 10 - (vbat / 100) * 10;
	line1[3] = '0' + vbat - (vbat / 10) * 10;
#endif
#ifdef POWERMETER
	intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV);
	line1[8] = '0' + intPowerMeterSum / 10000;
	line1[9] = '0' + intPowerMeterSum / 1000 - (intPowerMeterSum / 10000) * 10;
	line1[10] = '0' + intPowerMeterSum / 100 - (intPowerMeterSum / 1000) * 10;
	line1[11] = '0' + intPowerMeterSum / 10 - (intPowerMeterSum / 100) * 10;
	line1[12] = '0' + intPowerMeterSum - (intPowerMeterSum / 10) * 10;
	//line2[13] = '0'+powerTrigger1/100; line2[14] = '0'+powerTrigger1/10-(powerTrigger1/100)*10; line2[15] = '0'+powerTrigger1-(powerTrigger1/10)*10;
#endif
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(1);
	LCDprintChar(line1);	//refresh line 1 of LCD
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(2);		//position on line 2 of LCD
#ifdef VBAT
	LCD_BAR(7, (((vbat - VBATLEVEL1_3S) * 100) / VBATREF));
	LCDprintChar("  ");
#endif
#ifdef POWERMETER
	//     intPowerMeterSum = (pMeter[PMOTOR_SUM]/PLEVELDIV);
	//   pAlarm = (uint32_t) powerTrigger1 * (uint32_t) PLEVELSCALE * (uint32_t) PLEVELDIV; // need to cast before multiplying
	if (powerTrigger1)
	    LCD_BAR(7, (intPowerMeterSum / powerTrigger1 * 2));	// bar graph powermeter (scale intPowerMeterSum/powerTrigger1 with *100/PLEVELSCALE)
#endif
	break;
    case 'A':			// button A on Textstar LCD -> angles 
	uint16_t unit;
	strcpy(line1, "Deg ___._  ___._");
	/*            0123456789.12345 */
	strcpy(line2, "___,_A max___,_A");	//uin16_t cycleTimeMax
	if (angle[0] < 0) {
	    unit = -angle[0];
	    line1[3] = '-';
	} else
	    unit = angle[0];
	line1[4] = '0' + unit / 1000;
	line1[5] = '0' + unit / 100 - (unit / 1000) * 10;
	line1[6] = '0' + unit / 10 - (unit / 100) * 10;
	line1[8] = '0' + unit - (unit / 10) * 10;
	if (angle[1] < 0) {
	    unit = -angle[1];
	    line1[10] = '-';
	} else
	    unit = angle[1];
	line1[11] = '0' + unit / 1000;
	line1[12] = '0' + unit / 100 - (unit / 1000) * 10;
	line1[13] = '0' + unit / 10 - (unit / 100) * 10;
	line1[15] = '0' + unit - (unit / 10) * 10;
#ifdef LOG_VALUES
	unit = powerAvg * PINT2mA;
	line2[0] = '0' + unit / 10000;
	line2[1] = '0' + unit / 1000 - (unit / 10000) * 10;
	line2[2] = '0' + unit / 100 - (unit / 1000) * 10;
	line2[4] = '0' + unit / 10 - (unit / 100) * 10;
	unit = powerMax * PINT2mA;
	line2[10] = '0' + unit / 10000;
	line2[11] = '0' + unit / 1000 - (unit / 10000) * 10;
	line2[12] = '0' + unit / 100 - (unit / 1000) * 10;
	line2[14] = '0' + unit / 10 - (unit / 100) * 10;
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(2);
	LCDprintChar(line2);	//refresh line 2 of LCD
#endif
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(1);
	LCDprintChar(line1);	//refresh line 1 of LCD
	break;
    case 'D':			// button D on Textstar LCD -> sensors
#define GYROLIMIT 20		// threshold: for larger values replace bar with dots
#define ACCLIMIT 30		// threshold: for larger values replace bar with dots      LCDprint(0xFE);LCDprint('L');LCDprint(1);LCDprintChar("G "); //refresh line 1 of LCD
	if (abs(gyroData[0]) < GYROLIMIT) {
	LCD_BAR(4, (GYROLIMIT + gyroData[0]) * 50 / GYROLIMIT)} else
	    LCDprintChar("....");
	LCDprint(' ');
	if (abs(gyroData[1]) < GYROLIMIT) {
	LCD_BAR(4, (GYROLIMIT + gyroData[1]) * 50 / GYROLIMIT)} else
	    LCDprintChar("....");
	LCDprint(' ');
	if (abs(gyroData[2]) < GYROLIMIT) {
	LCD_BAR(4, (GYROLIMIT + gyroData[2]) * 50 / GYROLIMIT)} else
	    LCDprintChar("....");
	LCDprint(0xFE);
	LCDprint('L');
	LCDprint(2);
	LCDprintChar("A ");	//refresh line 2 of LCD
	if (abs(accSmooth[0]) < ACCLIMIT) {
	LCD_BAR(4, (ACCLIMIT + accSmooth[0]) * 50 / ACCLIMIT)} else
	    LCDprintChar("....");
	LCDprint(' ');
	if (abs(accSmooth[1]) < ACCLIMIT) {
	LCD_BAR(4, (ACCLIMIT + accSmooth[1]) * 50 / ACCLIMIT)} else
	    LCDprintChar("....");
	LCDprint(' ');
	if (abs(accSmooth[2] - acc_1G) < ACCLIMIT) {
	LCD_BAR(4, (ACCLIMIT + accSmooth[2] - acc_1G) * 50 / ACCLIMIT)} else
	    LCDprintChar("....");
	break;
    }				// end switch (telemetry) 
}				// end function
#endif				//  LCD_TELEMETRY
